/*
* Copyright (c) 2011 adVolition
*
* Permission is hereby granted, free of charge, to any person
* obtaining a copy of this software and associated documentation
* files (the "Software"), to deal in the Software without
* restriction, including without limitation the rights to use,
* copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following
* conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* Except as contained in this notice, the name(s) of the above
* copyright holders shall not be used in advertising or otherwise to
* promote the sale, use or other dealings in this Software without prior
* written authorization.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
* OTHER DEALINGS IN THE SOFTWARE.
 */

package ArenaSimulator;
import java.lang.Math.*;
import com.thoughtworks.xstream.annotations.*;

/**
 * This is a class for constructing and processing two-dimentional vectors.
 */
@XStreamAlias("vertex") public class Vector2D {
    @XStreamAlias("x") @XStreamAsAttribute private Double _x;
    @XStreamAlias("y") @XStreamAsAttribute private Double _y;

    private transient Double _amplitude;

    public Vector2D(){
        _x = 0.0;
        _y = 0.0;
        _amplitude = 0.0;
    }

    public Vector2D(Double x, Double y) {
        _x = x;
        _y = y;
        computeAmplitude();
    }

    public Object readResolve() {
        if (_x == null) _x = 0.0;
        if (_y == null) _y = 0.0;
        computeAmplitude();
        return this;
    }

    /**
     * Defines the vector using Cartesian coordinates.
     * @param x
     * @param y
     */
    public void cartesianSet(Double x, Double y) {
        _x = x;
        _y = y;
        computeAmplitude();
    }

    /**
     * To save time, this function computes and stores the vector's amplitude
     * whenever changes are made to it's x or y component.
     * @return
     */
    private void computeAmplitude() {
        _amplitude = Math.sqrt(
                Math.pow(_x, 2.0) +
                Math.pow(_y, 2.0)
                );
    }

    /**
     * Converts degrees to radians.
     * @param degrees
     * @return
     */
    private double computeRadians(Double degrees) {
        return degrees / 180.0 * Math.PI;
    }

    /**
     * Returns a new vector which is an exact copy of the current vector.
     * @return
     */
    public Vector2D copy() {
        Vector2D copiedVector = new Vector2D();
        copiedVector.vectorSet(this);
        return copiedVector;
    }

    public Boolean equals(Vector2D testVector) {
        if(_x == testVector.getX() && _y == testVector.getY()) {
            return true;
        }
        return false;
    }

    /**
     * Returns the distance from another vector point.
     * @param point
     * @return
     */
    public Double getDistanceToPoint(Vector2D point) {
        Vector2D distance = new Vector2D();
        distance.vectorSet(point);
        distance.subtractVector(this);
        return distance.getAmplitude();
    }

    /**
     * Returns the distance from a line segment defined by point A and point B.
     * @param pointA
     * @param pointB
     * @return
     */
    public Double getDistanceToLine(Vector2D pointA, Vector2D pointB) {
        
        Vector2D vectorAP = new Vector2D();
        vectorAP.vectorSet(this);
        vectorAP.subtractVector(pointA);

        Vector2D vectorBP = new Vector2D();
        vectorBP.vectorSet(this);
        vectorBP.subtractVector(pointB);

        Vector2D vectorAB = new Vector2D();
        vectorAB.vectorSet(pointB);
        vectorAB.subtractVector(pointA);

        Vector2D vectorBA = new Vector2D();
        vectorBA.vectorSet(pointA);
        vectorBA.subtractVector(pointB);

        Double projectionAPonAB = vectorAP.getProjectionOn(vectorAB);
        Double projectionBPonBA = vectorBP.getProjectionOn(vectorBA);

        if (projectionAPonAB > 0.0 && projectionBPonBA > 0.0) {
            // Return the distance to the line if vector is between the line ends.
            return Math.sqrt(Math.pow(vectorAP.getAmplitude(), 2) - Math.pow(projectionAPonAB, 2));
        } else {
            // Otherwise, return the distance to the nearest point.
            Double amplitudeAP = vectorAP.getAmplitude();
            Double amplitudeBP = vectorBP.getAmplitude();
            if (amplitudeAP < amplitudeBP) {
                return amplitudeAP;
            } else {
                return amplitudeBP;
            }
        }
    }

    /**
     * Defines the vector using polar coordinates.
     * @param angle
     * @param radius
     */
    public void polarSet(Double radius, Double angle) {
        Double radians = computeRadians(angle);
        _x = radius * Math.sin(radians);
        _y = radius * Math.cos(radians);
        computeAmplitude();
    }

    /**
     * Sets the vector using the parameters from the new vector.
     * @param newVector
     */
    public void vectorSet(Vector2D newVector) {
        _x = newVector.getX();
        _y = newVector.getY();
        _amplitude = newVector.getAmplitude();
    }

    /**
     * Returns the vector's x coordinate.
     * @return
     */
    public Double getX() {
        return _x;
    }

    /**
     * Returns an int value for x adjusted by the graphic display scale.
     * @param scale (centemeters/pixel)
     * @return
     */
    public int getScaledX(Double scale) {
        return (int) (_x / scale);
    }

    /**
     * Returns the y coordinate of the vector.
     * @return
     */
    public Double getY() {
        return _y;
    }

    /**
     * Returns an int value for y adjusted by the graphic display scale.
     * @param scale (centemeters/pixel)
     * @return
     */
    public int getScaledY(Double scale) {
        return (int) (_y / scale);
    }

    /**
     * Returns the vector's amplitude.
     * @return
     */
    public Double getAmplitude() {
        return _amplitude;
    }

    /**
     * Divides the vector by a scalar divisor.
     * @param divisor
     */
    public void divideBy(Double divisor) {
        _x /= divisor;
        _y /= divisor;
        computeAmplitude();
    }

    /**
     * Multiplies the vector by a scalar factor.
     * @param factor
     */
    public void multiplyBy(Double factor) {
        _x *= factor;
        _y *= factor;
        computeAmplitude();
    }

    /**
     * Subtracts another vector from the current vector.
     * @param vector
     */
    public void subtractVector(Vector2D vector) {
        _x -= vector.getX();
        _y -= vector.getY();
        computeAmplitude();
    }


    /**
     * Adds another vector to the current vector.
     * @param vector
     */
    public void addVector(Vector2D vector) {
        _x += vector.getX();
        _y += vector.getY();
        computeAmplitude();
    }

    /**
     * Normalizes the current vector, setting its amplitude to 1.0 without affecting
     * its direction.
     */
    public void normalize() {
        if (_amplitude > 0.0) {
            divideBy(_amplitude);
        }
    }

    /**
     * Returns the dot product of this vector and a second vector.
     * @param secondVector
     * @return
     */
    public Double getDotProduct(Vector2D secondVector) {
        return (_x * secondVector.getX() + _y * secondVector.getY());
    }

    /**
     * Returns the component of the vector that lies on the axis defined
     * by the target vector.
     * @param target
     * @return
     */
    public Double getProjectionOn(Vector2D target) {
        return getDotProduct(target) / target.getAmplitude();
    }

    public Double getCrossProduct(Vector2D secondVector) {
        return (_x * secondVector.getY() + _y * secondVector.getX());
    }

    /**
     * Converts the vector to a new coordingate system.
     * The new coordinate system defined by specifying a new origin
     * and a point on the new y axis.
     * This function is used to convert the vector to a coordinate system in which
     * the robot is centered at the origin and facing along the y axis.
     * @param origin
     * @param xAxis
     * @param yAxis
     * @return
     */
    public void transformCoordinates(Vector2D origin, Vector2D xAxis, Vector2D yAxis) {
        
        subtractVector(origin);

        Double x = getProjectionOn(xAxis);
        Double y = getProjectionOn(yAxis);

        _x = x;
        _y = y;
        computeAmplitude();
    }
}
